#include <iostream>
#include <ros/ros.h>
#include <string.h>
#include "SCServo.h"
#include <geometry_msgs/Vector3Stamped.h>
#include <geometry_msgs/PoseStamped.h>
#include "std_msgs/Float64.h"
#include <eigen3/Eigen/Dense>
#include <signal.h>

using namespace std;

/* -------------------------------------------------------------------------- */
/*                                 Global Var                                 */
/* -------------------------------------------------------------------------- */

constexpr u8 SERVO_ID{1};
constexpr u16 SPEED{100};
constexpr double ANGLE2SERVO = 4096.0 / 360.0;
constexpr double SERVO2ANGLE = 360.0 / 4096.0;
constexpr double ANGLE2RAD = M_PI / 180.0;
constexpr double SERVO2RAD = SERVO2ANGLE * ANGLE2RAD;
constexpr double RAD2SERVO = 1.0 / SERVO2RAD;
// speed in 50 step/s = 50 / 4096 * 60 rpm
constexpr double SPEED2RAD = 50.0 / 4096.0 * 2.0 * M_PI;

SMSBL servo_serial;

ros::Publisher servo_state_pub;
ros::Publisher camera_pos_pub;

Eigen::Matrix3d m_base2cam;
Eigen::Vector3d t_body2base_body{0.0, 0.0, 0.0};

/* -------------------------------------------------------------------------- */
/*                                  function                                  */
/* -------------------------------------------------------------------------- */

void signal_handler(int signo)
{
  servo_serial.end();
  ROS_INFO("shutdown servo_read_write node");
  ros::shutdown();
}

void set_angle_cb(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
  // add minus sign to make the servo rotate in the right direction
  s16 pos = (-msg->pose.position.x + 180) * ANGLE2SERVO;
  servo_serial.WritePosEx(SERVO_ID, pos, SPEED);
}

void set_rad_cb(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
  // add minus sign to make the servo rotate in the right direction
  s16 pos = (-msg->pose.position.x + M_PI) * RAD2SERVO;
  servo_serial.WritePosEx(SERVO_ID, pos, SPEED);
}

void pub_pos_cb(const geometry_msgs::PoseStampedConstPtr &msg)
{
  if (servo_serial.FeedBack(SERVO_ID) == -1)
    ROS_WARN_STREAM_THROTTLE(0.2, "servo_serial.FeedBack == -1");

  geometry_msgs::PoseStamped cam_pose;
  geometry_msgs::Vector3Stamped servo_state_msg;
  cam_pose.header.stamp = ros::Time::now();
  servo_state_msg.header.stamp = cam_pose.header.stamp;

  double rad = M_PI - servo_serial.ReadPos(SERVO_ID) * SERVO2RAD;
  double vel = servo_serial.ReadSpeed(SERVO_ID) * SPEED2RAD;

  servo_state_msg.vector.x = rad;
  servo_state_msg.vector.y = vel;
  servo_state_pub.publish(servo_state_msg);

  Eigen::Matrix3d m_body2base =
      Eigen::AngleAxisd(rad, Eigen::Vector3d(0.0, 0.0, 1.0)).toRotationMatrix();
  Eigen::Quaterniond q_world2body =
      Eigen::Quaterniond(msg->pose.orientation.w,
                         msg->pose.orientation.x,
                         msg->pose.orientation.y,
                         msg->pose.orientation.z);
  Eigen::Quaterniond q_world2cam =
      q_world2body * Eigen::Quaterniond(m_body2base * m_base2cam);

  Eigen::Vector3d t_body2base_world = q_world2body * t_body2base_body;

  cam_pose.pose.position.x = msg->pose.position.x + t_body2base_world(0);
  cam_pose.pose.position.y = msg->pose.position.y + t_body2base_world(1);
  cam_pose.pose.position.z = msg->pose.position.z + t_body2base_world(2);
  cam_pose.pose.orientation.w = q_world2cam.w();
  cam_pose.pose.orientation.x = q_world2cam.x();
  cam_pose.pose.orientation.y = q_world2cam.y();
  cam_pose.pose.orientation.z = q_world2cam.z();
  camera_pos_pub.publish(cam_pose);
}

void servo_pub_timer_callback(const ros::TimerEvent &e)
{
  if (servo_serial.FeedBack(SERVO_ID) == -1)
    ROS_WARN_STREAM_THROTTLE(0.2, "servo_serial.FeedBack == -1");

  geometry_msgs::Vector3Stamped servo_state_msg;
  servo_state_msg.header.stamp = ros::Time::now();
  servo_state_msg.vector.x = 180 - servo_serial.ReadPos(SERVO_ID) * SERVO2ANGLE;
  // speed in 50 step/s = 50 / 4096 * 60 rpm
  servo_state_msg.vector.y = servo_serial.ReadSpeed(SERVO_ID) * SPEED2RAD;
  servo_state_pub.publish(servo_state_msg);
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "servo_read_write");
  ros::NodeHandle nh;

  signal(SIGINT, signal_handler);

  std::string port;
  bool whether_pub_servo_state;
  int pub_rate;

  const std::string &node_name = ros::this_node::getName();
  nh.param(node_name + "/" + "servo/serial_port", port, std::string("/dev/ttyUSB0"));
  nh.param(node_name + "/" + "servo/pub_state", whether_pub_servo_state, true);
  nh.param(node_name + "/" + "servo/pub_rate", pub_rate, 50);
  nh.param(node_name + "/" + "servo/x_off", t_body2base_body(0), 0.0);
  nh.param(node_name + "/" + "servo/y_off", t_body2base_body(1), 0.0);
  nh.param(node_name + "/" + "servo/z_off", t_body2base_body(2), 0.0);

  ROS_INFO_STREAM("[SERVO] offset: " << t_body2base_body.transpose());

  if (!servo_serial.begin(115200, port.c_str())) {
    ROS_ERROR_STREAM("Failed to init smsbl motor! " << argv[1]);
    return 0;
  }

  ros::Subscriber set_angle_sub =
      nh.subscribe<geometry_msgs::PoseStamped>("/servo/set_angle", 1, &set_angle_cb);
  ros::Subscriber set_rad_sub =
      nh.subscribe<geometry_msgs::PoseStamped>("/servo/set_rad", 1, &set_rad_cb);

  ros::Subscriber cam_base_pose_sub =
      nh.subscribe<geometry_msgs::PoseStamped>("/cam_base_pose", 1, &pub_pos_cb);

  servo_state_pub = nh.advertise<geometry_msgs::Vector3Stamped>("/servo/state", 1);
  camera_pos_pub = nh.advertise<geometry_msgs::PoseStamped>("/camera_pose", 1);

  ros::Timer servo_pub_timer;
  if (whether_pub_servo_state) {
    servo_pub_timer =
        nh.createTimer(ros::Duration(1 / pub_rate), servo_pub_timer_callback);
  }

  // init
  m_base2cam << 0.0, 0.0, 1.0,
      -1.0, 0.0, 0.0,
      0.0, -1.0, 0.0;

  while(ros::ok()) {
    ros::Duration(0.01).sleep();
    ros::spinOnce();
  }

  return 0;
}
